{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# DoorHinge joint inspector\n",
    "The `DoorHinge` class is a `ForceElement`, which models a revolute door hinge joint that could exhibits different force/torque characterisitcs at different states due to the existence of different type of torques on the joint. This class implements a \"christmas tree\" accumulation of these different torques (i.e. function approximation) in an empirical and unprincipled way. Specifically, different curves are assigned to different torques to mimic their evolution based on the joint state and some prespecified parameters. More details can be referred to the doxygen documentation of the `DoorHinge` class.\n",
    "\n",
    "\n",
    "This tutorial provides a small tool to allow you to play with the `DoorHingeConfig` parameter and visualize the properties of a particular setup. It could facilitate you get the specific kind of door hinge joint behavior you are interested. In this tutorial, we provide a common dishwasher door modeling as an example."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A common dishwasher door has a frictional torque sufficient for it to rest motionless at any angle, a catch at the top to hold it in place, a dashpot (viscous friction source) to prevent it from swinging too fast, and a spring to counteract some of its mass. Two figures are provided to illustrate the dishwasher door hinge torque with the given default parameters. We first used a force gauge to estimate these values roughly. Then we simulate the dishwasher door for minor adjustment."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from pydrake.multibody.plant import MultibodyPlant\n",
    "from pydrake.multibody.tree import (\n",
    "    DoorHingeConfig, DoorHinge, RevoluteJoint\n",
    ")\n",
    "\n",
    "# Build a simple MultibodyPlant that contains two bodies and a revolute joint.\n",
    "# Then add a DoorHinge ForceElement to the joint.\n",
    "plant = MultibodyPlant(0.0)\n",
    "\n",
    "body_a = plant.AddRigidBody(name=\"body_a\")\n",
    "body_b = plant.AddRigidBody(name=\"body_b\")\n",
    "\n",
    "revolute_joint = plant.AddJoint(RevoluteJoint(\n",
    "        name=\"revolve_joint\", frame_on_parent=body_a.body_frame(),\n",
    "        frame_on_child=body_b.body_frame(), axis=[0, 0, 1],\n",
    "        damping=0.0))\n",
    "\n",
    "# Create a default DoorHingeConfig. The user should change the parameters before\n",
    "# calling AddForceElement.\n",
    "door_hinge_config = DoorHingeConfig()\n",
    "door_hinge_config.spring_constant = 3.0\n",
    "\n",
    "# Add the DoorHinge and finalize the plant.\n",
    "door_hinge = plant.AddForceElement(DoorHinge(\n",
    "            joint=revolute_joint, config=door_hinge_config))\n",
    "plant.Finalize()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Prepare the data for plotting.\n",
    "min_q = 0.0\n",
    "max_q = np.deg2rad(80)\n",
    "min_v = -0.1\n",
    "max_v = 0.1\n",
    "step_size = 0.0005\n",
    "q_array = np.arange(min_q, max_q, step_size)\n",
    "v_array = np.arange(min_v, max_v, step_size)\n",
    "\n",
    "# Plot the torque over angle at zero velocity\n",
    "torque_zero_velocity = [\n",
    "    door_hinge.CalcHingeTorque(q, 0.0)\n",
    "    for q in q_array\n",
    "]\n",
    "\n",
    "# Plot the torque over the angular velocity at 30 degree angle\n",
    "torque_30_angle = [\n",
    "    door_hinge.CalcHingeTorque(np.deg2rad(30), v)\n",
    "    for v in v_array\n",
    "]\n",
    "plt.plot(v_array, torque_30_angle)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first figure shows the static characteristic of the dishwasher door. At $q = 0^{\\circ}$, there exists a negative catch torque to prevent the door from moving. After that, the torsional spring torque will dominate to compensate part of the door gravity. The second figure shows the dynamic feature of the dishwasher door at $q = 30^{\\circ}$. Whenever the door tries to move, there will be a counter torque to prevent that movement, which therefore keeps the door at rest. Note that, due to the gravity, the dishwasher door will be fully open eventually. However, this process can be really slow because of the default `motion_threshold` is set to be very small."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
